// -----------------------------------------------------------------------------
// M A R I T I M E   R E S E A R C H   I N S T I T U T E   N E T H E R L A N D S
// -----------------------------------------------------------------------------
// Program  : preparea
// File     : preparea.cpp
// -----------------------------------------------------------------------------
// Revisions:
// Februari 13, 2009  asicherer
// -----------------------------------------------------------------------------
// utility to define a path for interactive waves tests
// -----------------------------------------------------------------------------

// note: as this is testbed-only code the architecture is not to be taken as an example

#error only make with plot=1
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <vector>
#include <deque>
#include <sys/time.h> // gettimeofday
#include <sys/stat.h> // stat
#ifndef _WINDOWS
#include "mermaid.h"
#endif
#include "FloatUtils.h"
#include "StringUtils.h"
#include "Array2D.h"
#include "InteractiveWaves.h"
#include "FileReader.h"
#include "Interpolator.h"
#include "OsgPlotter.h"
#include "PrepWindow.h"
#include "FileUtils.h"

#define NRP               4  // maximum number of pulses
#define WAYPOINT_SIZE    20  // size of waypoint spheres
#define PATHPOINT_SIZE   7  // size of path spheres
#define WAYPOINT_COLOR   "red" 
#define PATHPOINT_COLOR  "yellow" 
#define WAYPOINT_SHAPE   "sphere" 
#define PATHPOINT_SHAPE  "sphere" 
#define STARTPOINT_COLOR "green" 
#define STARTPOINT_SHAPE "capsule"

struct Point
{
    float x1, x2;
    Point(float a, float b) : x1(a), x2(b) {}
};
typedef std::deque<Point> Waypoints;

Waypoints waypoints[NRP];

static int steps;            // number of simulation steps
static int skips;            // visualizion per 'skips' steps
static std::string areafile; // file with area data
static std::string testfile; // file with testbed data
static std::string caption;
static float timestep;       // duration of one step in seconds

static PrepWindow *pPrepWindow = NULL;
static OsgPlotter *pPlotterBathymetry = NULL;

static int nrp;                        // number of pressure pulses
static float r1[NRP], r2[NRP];         // radius of pressure pulse
static float x1[NRP], x2[NRP];         // start position of pressure pulse
static float v1[NRP], v2[NRP];         // start velocity of pressure pulse
static float a1[NRP], a2[NRP];         // start acceleration of pressure pulse
static Waypoints path[NRP];

static int   Nx1, Nx2;       // size in points in x1,x2 direction
static float dx1, dx2;       // grid step size in x1,x2 direction
static float h0c;
static Array2D<float> *ph0;

static bool changed = false;
static bool pathVisible = true;
static bool refresh = false;

static void computePath(int ship)
{
    while (pPlotterBathymetry->removeObject("1")) {}
    path[ship].clear();
    unsigned int n = waypoints[ship].size();
    if (n > 0 && pathVisible)
    {
        pPlotterBathymetry->addObject("1", STARTPOINT_SHAPE, STARTPOINT_COLOR, PATHPOINT_SIZE, 
                                      waypoints[ship][0].x1,
                                      waypoints[ship][0].x2, 1,
                                      waypoints[ship][0].x1 - 4 * v1[ship], 
                                      waypoints[ship][0].x2 - 4 * v2[ship], 1);
    }
    Array1D<double> x(n);
    Array1D<double> y(n);
    for (unsigned int i = 0; i < n; ++i)
    {
        x[i] = waypoints[ship][i].x1;
        y[i] = waypoints[ship][i].x2;
        if (i > 0 && pathVisible)
        {
            pPlotterBathymetry->addObject("1", WAYPOINT_SHAPE, WAYPOINT_COLOR, 
                                               WAYPOINT_SIZE, x[i], y[i], 1);
        }
    }
    if (n < 2)
    {
        return;
    }
    Interpolator ip(x, y);
    Array1D<double> xx, yy;
    int nn = std::max(Nx1, Nx2);
    ip.interpolate(nn, &xx, &yy);
    for (int i = 0; i < nn; ++i)
    {
        path[ship].push_back(Point(xx[i], yy[i]));
        if (pathVisible)
        {
            pPlotterBathymetry->addObject("1", PATHPOINT_SHAPE, PATHPOINT_COLOR,
                                               PATHPOINT_SIZE, xx[i], yy[i], 1);
        }
    }
}

static void writePaths()
{
    FILE *fp = fopen(testfile.c_str(), "r");
    FILE *ofp = fopen("temptest", "w");
    char buffer[101];
    bool copy = true;
    while (copy && fgets(buffer, 100, fp))
    {
        if (copy)
        {
            fprintf(ofp, "%s", buffer);
            if (!strncmp(buffer, "[PulsePaths]", 12))
            {
                copy = false;
            }
        }
    }
    if (copy)
    {
        fprintf(ofp,"[PulsePaths]\n");
    }
    for (int i = 0; i < nrp; ++i)
    {
        fprintf(ofp, "      %u        # waypoints for pulse %d\n", (unsigned int)(waypoints[i].size() - 1), i + 1);
        for (unsigned int j = 1; j < waypoints[i].size(); ++j) // don't save computed (first) one
        {
            fprintf(ofp, "%7.2f %7.2f\n", waypoints[i][j].x1, waypoints[i][j].x2);
        }
    }
    fclose(fp);
    fclose(ofp);
    rename("temptest", testfile.c_str());
}

static void computeFirstWaypoint(int ship)
{
    float xx = Nx1 * dx1;
    float yy = Nx2 * dx2;
    float wpx = -1;
    float wpy = -1;
    int n = steps ? steps : 10000;
    for (int s = 0; s < n; ++s)
    {
        float t = s * timestep;
        float x = x1[ship] + v1[ship] * t + a1[ship] * FloatUtils::square(t);
        float y = x2[ship] + v2[ship] * t + a2[ship] * FloatUtils::square(t);
        if (x > 0 && x < xx && y > 0 && y < yy)
        {
            wpx = x;
            wpy = y;
            break;
        }
    }
    waypoints[ship].push_front(Point(wpx, wpy));
}



// callbacks for PrepWindow

/// sets exaggerated z scale
void setZscale(float scale)
{
    pPlotterBathymetry->setZscale(scale);
}

/// clears waypoints and path for specified ship
void clearPathDefinition(int ship)
{
    pPrepWindow->setWaypointLabel("0");
    while (pPlotterBathymetry->removeObject("1")) {}
    path[ship].clear();
    waypoints[ship].clear();
    computeFirstWaypoint(ship);
    computePath(ship);
    changed = true;
}

/// selects specified ship
void selectShip(int ship)
{
    if (pathVisible)
    {
        computePath(ship);
    }
}

/// saves waypoints to file
void savePathDefinition()
{
    puts("save");
    writePaths();
    changed = false;
}

/// reloads waypoints from file
void refreshPathDefinition()
{
    puts("load");
    refresh = true;
    changed = false;
}

/// closes application
void quitPathDefinition()
{
    bool save = false;
    if (changed)
    {
        if (yesnoBox("Caution", "Save path data?", &save))
        {
            if (save)
            {
                savePathDefinition();
            }
            exit(0);
        }
    }
    else
    {
        exit(0);
    }
}

/// returns number of waypoints of specified ship
int getNumberOfWaypoints(int ship)
{
    if (ship <= nrp)
    {
        return waypoints[ship].size();
    }
    return 0;
}

/// handles left clicks on heightfield
static bool leftClickHandler(const osg::Vec3& xyz, const std::string& name, bool)
{
    int ship = pPrepWindow->getShip();
    waypoints[ship].push_back(Point(xyz.x(), xyz.y()));
    computePath(ship);
    pPrepWindow->setWaypointLabel("%d", waypoints[ship].size());
    changed = true;
    return true;
}

/// handles right clicks on heightfield
static bool rightClickHandler(const osg::Vec3& xyz, const std::string& name, bool)
{
    char buffer[80];
    sprintf(buffer, "%s   %G m", testfile.c_str(), xyz.z());
    caption = buffer;
    return true;
}

/// partially reads test file
static bool setup()
{
    try 
    {
        FileReader fr(testfile.c_str());
        do
        {
            std::string section = fr.ReadString();
            if (section == "[Test]")
            { 
                steps    = fr.ReadInt();
                skips    = fr.ReadInt();
                areafile = fr.ReadString();
            }
            else if (section == "[TimeSteps]")
            { 
                timestep   = fr.ReadFloat();
            }
            else if (section == "[PressurePulse]")
            {
                /* smoothing = */ fr.ReadInt();
                nrp = fr.ReadInt();
                ASSERT(nrp < NRP, "too many pulses");
                for (int i = 0; i < nrp; ++i)
                {
                    r1[i] = fr.ReadFloat();
                    r2[i] = fr.ReadFloat();
                    x1[i] = fr.ReadFloat();
                    x2[i] = fr.ReadFloat();
                    v1[i] = fr.ReadFloat();
                    v2[i] = fr.ReadFloat();
                    a1[i] = fr.ReadFloat();
                    a2[i] = fr.ReadFloat();
                    /* pm[i] = */ fr.ReadFloat();
                    /* pulseFile = */ fr.ReadString();
                }
            }
            else if (section == "[PulsePaths]")
            {
                for (int i = 0; i < nrp; ++i)
                {
                   int nr = fr.ReadInt();
                    for (int j = 0; j < nr; ++j)
                    {
                        REAL x, y;
                        fr.ReadFloats(&x, &y);
                        waypoints[i].push_back(Point(x, y));
                    }
                }
            }
        } while (!fr.Eof());
        return true;
    } 
    catch (...)
    {
        return false;
    }
}

#define BUFLEN 200
static void ReadLine(FILE *fp, float *a, float *b, float *c, bool *d)
{
    char buffer[BUFLEN];
    char *words[4];
    char *ptr;
    char* ignore;
    ignore = fgets(buffer, BUFLEN, fp);
    ptr = StringUtils::trim(buffer);
    StringUtils::split(ptr, words, 2, '%');
    StringUtils::split(words[0], words, 4, ' ');
    *a = atof(StringUtils::trim(words[0]));
    *b = atof(StringUtils::trim(words[1]));
    *c = atof(StringUtils::trim(words[2]));
    *d = strcmp(StringUtils::trim(words[3]), "T") == 0;
}

/// partially reads hu file
static void iwSetupArea(FileReader &fr)
{
    Nx1  = fr.ReadInt();
    Nx2  = fr.ReadInt();
    dx1  = fr.ReadFloat();
    dx2  = fr.ReadFloat();
printf("%d %d %G %G\n", Nx1, Nx2, dx1, dx2);
    h0c  = fr.ReadFloat();
    float dummy = fr.ReadFloat();
    dummy = fr.ReadFloat();
    std::string spec = fr.ReadString();
    
    // allocate the required array 0:Nx1+1,0:Nx2+1
    ph0  = new Array2D<float>(Nx1 + 3, Nx2 + 3);

    // fill the depth and mean velocity fields
    if (spec == std::string("-")) // use constant depth and mean velocities
    {
        *ph0 = h0c;
    } 
    else // read depths from file
    {
        FILE *fp = fopen(spec.c_str(), "r");
        for (int i2 = 0; i2 <= Nx2 + 1; ++i2) 
        {
            for (int i1 = 0; i1 <= Nx1 + 1; ++i1) 
            {
                float a, b, c;
                bool d;
                ReadLine(fp, &a, &b, &c, &d);
                (*ph0)[i1][i2] = -a;
            }
        }
        fclose(fp);
    }
}


/// reads parameters from par file
static bool iwSetup(const char *parameterfile)
{
    try 
    {
        FileReader fr(parameterfile);
        do
        {
            std::string section = fr.ReadString();
            if (section == "[Area]")
            {
                iwSetupArea(fr);
            }
        } while (!fr.Eof());
        return true;
    } 
    catch (...)
    {
        return false;
    }
}


int main(int argc, char *argv[])
{
    if (argc < 2)
    {
        fprintf(stderr, "preppath testfile\n");
        exit(-1);

    }
    testfile = std::string(argv[1]);
    while (true)
    {
        refresh = false;
        if (!FileUtils::exists(testfile))
        {
            fprintf(stderr, "%s does not exist\n", testfile.c_str());
            exit(-2);
        }
        if (!setup())
        {
            fprintf(stderr, "error setup %s\n", testfile.c_str());
        }

        if (!FileUtils::exists(areafile))
        {
            fprintf(stderr, "%s does not exist\n", areafile.c_str());
            exit(-3);
        }
        if (!iwSetup(areafile.c_str()))
        {
            fprintf(stderr, "error setup %s\n", areafile.c_str());
        }

        Array2D<float> bathymetry(Nx1 + 3, Nx2 + 3);
        caption = testfile;

        if (!pPlotterBathymetry)
        {
            pPlotterBathymetry = new OsgPlotter(1, 1, Nx1, Nx2, dx1, dx2, bathymetry, caption);
            pPlotterBathymetry->setZscale(1);
            pPlotterBathymetry->setLeftClickHandler(leftClickHandler);
            pPlotterBathymetry->setRightClickHandler(rightClickHandler);
        }

        if (!pPrepWindow)
        {
            pPrepWindow = new PrepWindow(&argc, &argv, nrp);
        }
        else
        {
            pPrepWindow->setNumberOfShips(nrp);
        }

        for (int i = 0; i < nrp; ++i)
        {
            computeFirstWaypoint(i);
        }
        computePath(pPrepWindow->getShip());

        while (!refresh)
        {
            microsleep(50000); // 0.05 seconds delay to conserve processor time
            pPrepWindow->handleEvents();
            bathymetry = *ph0;
            pPlotterBathymetry->plot();
        }
        delete ph0;
        for (int i = 0; i < nrp; ++i)
        {
            waypoints[i].clear();
        }
    }
    delete pPrepWindow;
    delete pPlotterBathymetry;

    return 0;
}

